//
// Created by enemy1205 on 2021/8/16.
//
#include "Math.h"
using namespace cv;
using namespace std;
float Deg2Rad(float Deg) { return PI * Deg / 180.f; }//角度制转弧度制

float Rad2Deg(float Rad) { return Rad * 180.f / PI; }//角度制转弧度制
/*求距离*/
extern float getDistance(const Point2f &A, const Point2f &B) {
    float distance;
    distance = pow((A.x - B.x), 2) + pow((A.y - B.y), 2);
    return sqrt(distance);
}
/**
 * @brief 用来获得相机中心相对于装甲板中心的角度
 *
 * @param cameraMatrix 相机内参
 * @param distCoeff 畸变参数
 * @param center 图像中装甲板中心
 * @return x, y 方向夹角 -- 目标在图像右方，point.x 为正，目标在图像下方，point.y 为正
 */
Point2f calculateRelativeAngle(const Mat &cameraMatrix, const Mat &distCoeff, Point2f center)
{
    Mat tf_point(3, 1, CV_32F);
    Mat cameraMatrix_inverse;
    cameraMatrix.convertTo(cameraMatrix_inverse, CV_32F);
    cameraMatrix_inverse = cameraMatrix_inverse.inv();
    tf_point.at<float>(0) = center.x;
    tf_point.at<float>(1) = center.y;
    tf_point.at<float>(2) = 1;
    // 得到tan角矩阵
    Mat tf_result = cameraMatrix_inverse * tf_point;
//    cout<<Rad2Deg(atanf(tf_result.at<float>(0)))<<endl;
    // 从图像坐标系转换成世界坐标系角度
    return Point2f(Rad2Deg(atanf(tf_result.at<float>(0))),
                   Rad2Deg(atanf(tf_result.at<float>(1))));
}

/**
 * @brief 用来获得装甲板中心的像素坐标
 * @param _cameraMatrix 相机内参
 * @param distCoeff 畸变参数
 * @param center 图像中装甲板中心
 * @return x, y 坐标
 */
Point2f calculateRelativeCenter(const Mat &_cameraMatrix, const Mat &distCoeff, Point2f angle)
{
    float yaw = tanf(Deg2Rad(angle.x));
    float pitch = tanf(Deg2Rad(angle.y));
    Mat center_vector(3, 1, CV_32F);
    Mat cameraMatrix;
    _cameraMatrix.convertTo(cameraMatrix, CV_32F);
    center_vector.at<float>(0) = yaw;
    center_vector.at<float>(1) = pitch;
    center_vector.at<float>(2) = 1;
    //得到tan角矩阵
    Mat result = cameraMatrix * center_vector;

    return Point2f(result.at<float>(0),
                   result.at<float>(1));
}
/**
 * @brief 获取角度差
 * @param angle_1
 * @param angle_2
 * @return 角度差
 */
float getDeltaAngle(float angle_1, float angle_2)
{
    float delta_angle = angle_1 - angle_2;
    if (angle_1 > 150.f && angle_2 < -150.f)
    {
        delta_angle -= 360.f;
    }
    else if (angle_1 < -150.f && angle_2 > 150.f)
    {
        delta_angle += 360.f;
    }
    return fabs(delta_angle);
}